/*
 * LocalizationFilter.h
 *
 *  Created on: 06.12.2011
 *      Author: jan
 */

#ifndef LOCALIZATIONFILTER_H_
#define LOCALIZATIONFILTER_H_

#include "Particle.h"
#include "DoubleProb.h"
#include "Point.h"
#include "AbstractMap.h"
#include "Measurement.h"
#include "WeightedDistribution.h"

namespace std {

class LocalizationFilter {
public:
	int nmbr; // number of particles
	double particleweights;
	double heading;
	double norm;
	double resol;
	AbstractMap<DoubleProb> * m;
	WeightedDistribution<DoubleProb> * dist;
	vector<Particle<DoubleProb> > particles;
	vector<Particle<DoubleProb> > new_particles;

	void setHeading(double h);
	void create();
	void predict(double head,double speed);
	void normalize();
	void weight();
	void resample();
	void measure(Measurement m);
	double bresenham(Point p, Point t, bool wall);
	LocalizationFilter(int n, double h, double resol, AbstractMap<DoubleProb> * m):nmbr(n),particleweights(1.0/n),heading(h),norm(0),resol(resol),m(m),dist(NULL){}
	virtual ~LocalizationFilter();

};

} /* namespace std */
#endif /* LOCALIZATIONFILTER_H_ */
